#include "zf_common_headfile.h"
#include "duoJi.h"
#include "encoder.h"
#include "motor.h"
#include "pit.h"
#include "math.h"
#include "image.h"

double angle, Vx, Vy;
int8_t Speed = 5;
double pi = 3.1415926;

int main()

{
    clock_init(SYSTEM_CLOCK_600M);
    debug_init();
    motor_init();
    encoder_init();
    image_init();
    icm20602_init();

    pit_init1();

    // system_delay_ms(100);

    while (1)
    {
        ChuLi_image();

        ips200_draw_line(Mid_line[59], 59, Mid_line[59 - QIANZHAN], 59 - QIANZHAN, RGB565_BLUE);
        ips200_draw_line(Mid_line[59 - QIANZHAN], 59 - QIANZHAN, Mid_line[59 - 2 * QIANZHAN], 59 - 2 * QIANZHAN, RGB565_BLUE);
        ips200_show_gray_image(0, 0, Pixle_hb[0], LCDW, LCDH, LCDW, LCDH, 0);
        ips200_show_string(0, 100, "Threshold:");
        ips200_show_int(100, 100, Threshold, 3);

        ips200_show_string(0, 120, "ERR:");
        ips200_show_float(100, 120, ERR, 6, 6);

        ips200_show_string(0, 140, "YERR:");
        ips200_show_int(100, 140, YERR, 3);
    }
}
